﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using AForge;
using AForge.Controls;
using AForge.Math.Geometry;
using AForge.Imaging;
using AForge.Imaging.Filters;
using AForge.Video;
using AForge.Video.DirectShow;
using AForge.Vision;
using System.Reflection;
using AForge.Robotics.Lego;
using System.Drawing.Imaging;
using System.IO;
using System.Threading;

namespace nxt
{
    public partial class Main_and_Camera : Form
    {

        string d = "";
        private FilterInfoCollection videoDevices;
        EuclideanColorFiltering filter = new EuclideanColorFiltering();
        Color color = Color.Black;
        GrayscaleBT709 grayscaleFilter = new GrayscaleBT709();
        BlobCounter blobCounter = new BlobCounter();
        int range = 120;
        private string cPTemp;
        private string[] cP = new string[2];
        Control robot;
        List<AForge.Point> coordinates = new List<AForge.Point>();
        AForge.Point xy = new AForge.Point();
        int[] savedMovesValue = new int[50];
        string [] savedMovesType = new string[50];
        bool colorFindResult = false; //szint találat esetén true.
        int globalCounter = 0; // savedMovements,savedMovementsType indexe.
        int ballCounter = 0; //labda számláló
        int stepCount = 0; //switccase-hez helyzet jelző

        #region Constructor
        public Main_and_Camera()
        {
            InitializeComponent();
            blobCounter.MinWidth = 2;
            blobCounter.MinHeight = 2;
            blobCounter.FilterBlobs = true;
            blobCounter.ObjectsOrder = ObjectsOrder.Size;
            try
            {
                // enumerate video devices
                videoDevices = new FilterInfoCollection(FilterCategory.VideoInputDevice);

                if (videoDevices.Count == 0)
                    throw new ApplicationException();

                // add all devices to combo
                foreach (FilterInfo device in videoDevices)
                {
                    camerasCombo.Items.Add(device.Name);
                }

                camerasCombo.SelectedIndex = 0;
            }
            catch (ApplicationException)
            {
                camerasCombo.Items.Add("No local capture devices");
                videoDevices = null;
            }

            Bitmap b = new Bitmap(320, 240);
            // Rectangle a = (Rectangle)r;
            Pen pen1 = new Pen(Color.FromArgb(160, 255, 160), 3);
            Graphics g2 = Graphics.FromImage(b);
            pen1 = new Pen(Color.FromArgb(255, 0, 0), 3);
            g2.Clear(Color.White);
            g2.DrawLine(pen1, b.Width / 2, 0, b.Width / 2, b.Width);
            g2.DrawLine(pen1, b.Width, b.Height / 2, 0, b.Height / 2);
            pictureBox1.Image = (System.Drawing.Image)b;

        }
        #endregion

        #region ImageProcessing

        private void videoSourcePlayer1_NewFrame(object sender, ref Bitmap image)
        {
            Bitmap objectsImage = null;
            Bitmap mImage = null;
            mImage = (Bitmap)image.Clone();
            filter.CenterColor = new RGB(Color.FromArgb(color.ToArgb()));
            filter.Radius = (short)range;

            objectsImage = image;
            filter.ApplyInPlace(objectsImage);

            BitmapData objectsData = objectsImage.LockBits(new Rectangle(0, 0, image.Width, image.Height),
            ImageLockMode.ReadOnly, image.PixelFormat);
            UnmanagedImage grayImage = grayscaleFilter.Apply(new UnmanagedImage(objectsData));
            objectsImage.UnlockBits(objectsData);


            blobCounter.ProcessImage(grayImage);
            Rectangle[] rects = blobCounter.GetObjectsRectangles();

            if (rects.Length > 0)
            {

                foreach (Rectangle objectRect in rects)
                {
                    Graphics g = Graphics.FromImage(mImage);
                    using (Pen pen = new Pen(Color.FromArgb(160, 255, 160), 5))
                    {
                        g.DrawRectangle(pen, objectRect);
                    }

                    g.Dispose();
                }
            }

            image = mImage;
        }

        private void videoSourcePlayer3_NewFrame(object sender, ref Bitmap image)
        {
            Bitmap objectsImage = null;


            // set center colol and radius
            filter.CenterColor = new RGB(Color.FromArgb(color.ToArgb()));
            filter.Radius = (short)range;
            // apply the filter
            objectsImage = image;
            filter.ApplyInPlace(image);

            // lock image for further processing
            BitmapData objectsData = objectsImage.LockBits(new Rectangle(0, 0, image.Width, image.Height),
                ImageLockMode.ReadOnly, image.PixelFormat);

            // grayscaling
            UnmanagedImage grayImage = grayscaleFilter.Apply(new UnmanagedImage(objectsData));

            // unlock image
            objectsImage.UnlockBits(objectsData);

            // locate blobs 
            blobCounter.ProcessImage(grayImage);
            Rectangle[] rects = blobCounter.GetObjectsRectangles();

            if (rects.Length > 0)
            {
                Rectangle objectRect = rects[0];

                // draw rectangle around derected object
                Graphics g = Graphics.FromImage(image);

                using (Pen pen = new Pen(Color.FromArgb(160, 255, 160), 5))
                {
                    g.DrawRectangle(pen, objectRect);
                }
                g.Dispose();
                int objectX = objectRect.X + objectRect.Width / 2 - image.Width / 2;
                int objectY = image.Height / 2 - (objectRect.Y + objectRect.Height / 2);
                ParameterizedThreadStart t = new ParameterizedThreadStart(p);
                Thread aa = new Thread(t);
                aa.Start(rects[0]);
            }
            Graphics g1 = Graphics.FromImage(image);
            Pen pen1 = new Pen(Color.FromArgb(160, 255, 160), 3);
            g1.DrawLine(pen1, image.Width / 2, 0, image.Width / 2, image.Width);
            g1.DrawLine(pen1, image.Width, image.Height / 2, 0, image.Height / 2);
            g1.Dispose();
        }

        void p(object r)
        {
            try
            {

                Bitmap b = new Bitmap(pictureBox1.Image);
                Rectangle a = (Rectangle)r;
                Pen pen1 = new Pen(Color.FromArgb(160, 255, 160), 3);
                Graphics g2 = Graphics.FromImage(b);
                pen1 = new Pen(color, 3);
                // Brush b5 = null;
                SolidBrush b5 = new SolidBrush(color);
                //   g2.Clear(Color.Black);


                Font f = new Font(Font, FontStyle.Bold);
                g2.DrawString("o", f, b5, a.Location);
                g2.Dispose();
                pictureBox1.Image = (System.Drawing.Image)b;
                this.Invoke((MethodInvoker)delegate
                {
                    if (a.Location.X > 0 || a.Location.Y > 0)
                    {
                        xy.X = a.Location.X;
                        xy.Y = a.Location.Y;
                        coordinates.Add(xy);
                        richTextBox1.Text = a.Location.ToString() + "\n" + richTextBox1.Text + "\n"; ;
                        colorFindResult = true;
                        //stopCamera();
                    }
                });
            }
            catch (Exception faa)
            {
                Thread.CurrentThread.Abort();
            }


            Thread.CurrentThread.Abort();
        }
        #endregion

        #region Exit Button

        private void button4_Click(object sender, EventArgs e)
        {
            try
            {
                videoSourcePlayer2.SignalToStop();
                videoSourcePlayer2.WaitForStop();
                videoSourcePlayer3.SignalToStop();
                videoSourcePlayer3.WaitForStop();
                robot.Disconnect();
            }

            catch
            {

            }
            DialogResult dialogResult = MessageBox.Show("Are you sure?", "Exit", MessageBoxButtons.YesNo);
            if (dialogResult == DialogResult.Yes)
            {
                Application.Exit();
            }
        }
        #endregion

        #region comPort Button

        private void button14_Click(object sender, EventArgs e)
        {
            comPort cP_ = new comPort();
            cP_.ShowDialog();
        }
        #endregion

        #region ControlPanel Button

        private void button8_Click(object sender, EventArgs e)
        {
            ControlPanel controlPanel = new ControlPanel();
            controlPanel.ShowDialog();
        }
        #endregion

        #region Form Closing Code

        private void Main_and_Camera_FormClosing(object sender, FormClosingEventArgs e)
        {


            videoSourcePlayer2.SignalToStop();
            videoSourcePlayer2.WaitForStop();
            videoSourcePlayer3.SignalToStop();
            videoSourcePlayer3.WaitForStop();

            groupBox1.Enabled = true;
        }
        #endregion

        #region Start Camera Button

        private void button1_Click_1(object sender, EventArgs e)
        {
            startCamera();
        }
        #endregion

        #region Start Camera

        private void startCamera()
        {
            videoSourcePlayer2.SignalToStop();
            videoSourcePlayer2.WaitForStop();
            videoSourcePlayer3.SignalToStop();
            videoSourcePlayer3.WaitForStop();

            // videoDevices = null;
            VideoCaptureDevice videoSource = new VideoCaptureDevice(videoDevices[camerasCombo.SelectedIndex].MonikerString);
            videoSource.DesiredFrameSize = new Size(320, 240);
            videoSource.DesiredFrameRate = 12;

            videoSourcePlayer2.VideoSource = videoSource;
            videoSourcePlayer2.Start();
            videoSourcePlayer3.VideoSource = videoSource;
            videoSourcePlayer3.Start();

            //groupBox1.Enabled = false;

            
        }
        #endregion

        #region Stop Camera

        private void stopCamera()
        {
            videoSourcePlayer2.SignalToStop();
            videoSourcePlayer2.WaitForStop();
            videoSourcePlayer3.SignalToStop();
            videoSourcePlayer3.WaitForStop();
            
        }
        #endregion

        #region Stop Camera Button

        private void button2_Click_1(object sender, EventArgs e)
        {
            stopCamera();
        }
        #endregion

        #region Object/Color settings
 
        private void numericUpDown1_ValueChanged(object sender, EventArgs e)
        {
            range = Convert.ToInt32(numericUpDown1.Value);
        }

        private void numericUpDown2_ValueChanged(object sender, EventArgs e)
        {
            blobCounter.MaxWidth = Convert.ToInt32(numericUpDown2.Value);
        }

        private void numericUpDown3_ValueChanged(object sender, EventArgs e)
        {
            blobCounter.MinWidth = Convert.ToInt32(numericUpDown3.Value);
        }

        private void button3_Click(object sender, EventArgs e)
        {
            colorDialog1.ShowDialog();
            color = colorDialog1.Color;
        }
        #endregion

        #region Start NXT Button

        private void button11_Click(object sender, EventArgs e)
        {
            startNXT();
        }
        #endregion

        #region Manual Control Button
        private void button17_Click(object sender, EventArgs e)
        {
            Manualy_controller MC = new Manualy_controller();
            MC.ShowDialog();
        }
        #endregion

        #region Reset Color Field

        private void resetColorField()
        {
            Bitmap b = new Bitmap(320, 240);
            // Rectangle a = (Rectangle)r;
            Pen pen1 = new Pen(Color.FromArgb(160, 255, 160), 3);
            Graphics g2 = Graphics.FromImage(b);
            pen1 = new Pen(Color.FromArgb(255, 0, 0), 3);
            g2.Clear(Color.White);
            g2.DrawLine(pen1, b.Width / 2, 0, b.Width / 2, b.Width);
            g2.DrawLine(pen1, b.Width, b.Height / 2, 0, b.Height / 2);
            pictureBox1.Image = (System.Drawing.Image)b;
        }
        #endregion

        #region Reset Color Field Button

        private void button7_Click(object sender, EventArgs e)
        {
            resetColorField();
        }
        #endregion

        #region Go Home

        private void goHome()
        {
            listBox1.Items.Add("Go Home Started..");
            listBox1.TopIndex = listBox1.Items.Count - 1;
            //gohome számítás.
            do
            {
                try
                {
                    switch (savedMovesType[globalCounter])
                    {
                        case "up":
                            {
                                robot.up(20,100,savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;
                        
                        case "down":
                            {
                                robot.down(20, 100, savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;

                        case "left":
                            {
                                robot.left(20, 100, savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;

                        case "right":
                            {
                                robot.right(20, 100, savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;

                        case "forward":
                            {
                                robot.forward(20, 100, savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;

                        case "backward":
                            {
                                robot.backward(20, 100, savedMovesValue[globalCounter]);
                                globalCounter--;
                                Thread.Sleep(10000);
                            }
                            break;
                    }
                }
                catch (Exception)
                {
                    listBox1.Items.Add("Go Home Failed.");
                    listBox1.TopIndex = listBox1.Items.Count - 1;
                }
            }
            while (globalCounter == 0);
            //labda eldobása
            try
            {
                robot.robotic_arm_close_open(false);
                listBox1.Items.Add("Ball dropped.");
                listBox1.TopIndex = listBox1.Items.Count - 1;

            }
            catch (Exception)
            {
                listBox1.Items.Add("Ball dropping error.");
                listBox1.TopIndex = listBox1.Items.Count - 1;
            }

            listBox1.Items.Add("Go Home Finished.");
            listBox1.TopIndex = listBox1.Items.Count - 1;
            savedMovesType = null;
            savedMovesValue = null;
            globalCounter = 0;
            resetColorField();
            
        }
        #endregion

        #region startNXT Game Strategy

        private int gameStartegy(int bCounter, ref int gCounter)
        {
            return gameStartegy(bCounter + 1, ref gCounter);
        }

        private void startNXT()
        {
                listBox1.Items.Add("NXT Started...");
                listBox1.TopIndex = listBox1.Items.Count - 1;
                resetColorField();
                colorFindResult = false;
                globalCounter = 0;
                ballCounter = 0;
                //startCamera();

                listBox1.Items.Add("Going to home position...");
                listBox1.TopIndex = listBox1.Items.Count - 1;
                try
                {
                    robot.up(20, 100, 1100);// 40cmre fel.
                    Thread.Sleep(10000);
                    //robot.left(20, 100, 1000); //balra el széléig.
                    /*
                    oldal mozgásnál a korrekciós fv az elmozdulás értékének
                     * +20%-át adja hozzá az elmozdulás irányába eső motorok fordulatához
                     * -20%-ot von le az ellentétes oldal motor fordulatából
                     * pl.: 1000 fok balra:   800 fok: felsőB, alsóB   1200 fok: felsőA, felsőB
                    */
                    //első 4 paraméter a 4 motor forgási iránya/sebessége: felsőA, felsőB, alsóA, alsóB
                    //másik 4 paraméter pedig a motorok egyenkénti elfordulása fokban: felsőA, felsőB, alsóA, alsóB
                    robot.correction(20, -20, 20, -20, 800, 1200, 80, 1200);
                    Thread.Sleep(10000);
                    
                    //robot.backward(20, 100, 3850);//hátra fele home pontig.
                    //damil korrekció ide + 2000 nem valós fordulat.
                    //-FA, -FB, +AA, +AB: hátra
                    //+FA, +FB, -AA, -AB: előre
                    //+FA, -FB, +AA, -AB: jobbra
                    //-FA, +FB, -AA, +AB: ballra
                    robot.correction(-20, -20, 20, 20, 3935, 5625, 3935, 5625);
                    Thread.Sleep(15000);
                    listBox1.Items.Add("Going to home position finished");
                    /*
                     // +FA, +FB, -AA, -AB
                     robot.correction(20, 20, -20, -20, 7740, 10850, 7740, 10850); előre
                     robot.correction(-20, 20, 20, -20, 4160, 4160, 1125, 1125); jobbra
                     robot.correction(-20, -20, 20, 20, 10850, 7740, 10850, 7740); hátra
                     robot.correction(20, -20, 20, -20, 395, 730, 1965, 3600); balra(pályaközepéig)
                     robot.correction(20, 20, -20, -20, 0, 0, 0, 0); előre (kezdőpontra)
                     robot.correction(20, 20, -20, -20, 7875, 6750, 7875, 6750); goHomera
                    */
                    listBox1.TopIndex = listBox1.Items.Count - 1;
                }
                catch (Exception)
                {
                    listBox1.Items.Add("Going to home position failed");
                    listBox1.TopIndex = listBox1.Items.Count - 1;
                }
                try
                {
                    while(ballCounter != 5)    
                    {   
                        startCamera();

                        switch(stepCount)
                        {
                            case 0: // első pásztázási menet, előre
                                {
                                    if (colorFindResult == false)
                                    {
                                        try
                                        {
                                            robot.forward(20, 100, 1000); // 1000-t cserélni kell helyes értékre.
                                            savedMovesType[globalCounter] = "backward";
                                            savedMovesValue[globalCounter] = -1000; //lekell cserélni az adott motorfordulatra
                                            stepCount++;
                                            globalCounter++;
                                            Thread.Sleep(10000); //10mp t vár h befejezze a müveletet, változtatni kell.
                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Nxt move error.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                    }
                                    else
                                    {
                                        //itt megkell hivni a koordináta számítást h pontosan hol a labda.
                                        ballCounter++;
                                        stepCount = 0;
                                        listBox1.Items.Add(ballCounter + ". ball found!");
                                        listBox1.TopIndex = listBox1.Items.Count - 1;
                                        try
                                        {
                                            robot.robotic_arm_close_open(false);
                                            listBox1.Items.Add("Ball catched.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                            Thread.Sleep(10000);

                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Ball catching error");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                        goHome();
                                    }
                                }
                                break;

                            case 1:
                                {
                                    if (colorFindResult == false)
                                    {
                                        try
                                        {
                                            robot.right(20, 100, 1000); // 1000-t cserélni kell helyes értékre.
                                            savedMovesType[globalCounter] = "left"; //ellentétes értéket adjuk, a gohome miatt.
                                            savedMovesValue[globalCounter] = -1000; //lekell cserélni az adott motorfordulatra
                                            globalCounter++;
                                            stepCount++;
                                            Thread.Sleep(10000); //10mp t vár h befejezze a müveletet, változtatni kell.
                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Nxt move error.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                    }
                                    else
                                    {
                                        //itt megkell hivni a koordináta számítást h pontosan hol a labda.
                                        ballCounter++;
                                        stepCount = 0;
                                        listBox1.Items.Add(ballCounter + ". ball found!");
                                        listBox1.TopIndex = listBox1.Items.Count - 1;
                                        try
                                        {
                                            robot.robotic_arm_close_open(false);
                                            listBox1.Items.Add("Ball catched.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                            Thread.Sleep(10000);

                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Ball catching error");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                        goHome();
                                    }
                                }
                                break;

                            case 2:
                                {
                                    if (colorFindResult == false)
                                    {
                                        try
                                        {
                                            robot.backward(20, 100, 1000); // 1000-t cserélni kell helyes értékre.
                                            savedMovesType[globalCounter] = "forward"; //ellentétes értéket adjuk, a gohome miatt.
                                            savedMovesValue[globalCounter] = -1000; //lekell cserélni az adott motorfordulatra. itt is ellentétes érték kell.
                                            globalCounter++;
                                            stepCount++;
                                            Thread.Sleep(10000); //10mp t vár h befejezze a müveletet, változtatni kell.
                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Nxt move error.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                    }
                                    else
                                    {
                                        //itt megkell hivni a koordináta számítást h pontosan hol a labda.
                                        ballCounter++;
                                        stepCount = 0;
                                        listBox1.Items.Add(ballCounter + ". ball found!");
                                        listBox1.TopIndex = listBox1.Items.Count - 1;
                                        try
                                        {
                                            robot.robotic_arm_close_open(false);
                                            listBox1.Items.Add("Ball catched.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                            Thread.Sleep(10000);

                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Ball catching error");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                        goHome();
                                    }
                                }
                                break;

                            case 3:
                                {
                                    if (colorFindResult == false)
                                    {
                                        try
                                        {
                                            robot.right(20, 100, 1000); // 1000-t cserélni kell helyes értékre.
                                            savedMovesType[globalCounter] = "left"; //ellentétes értéket adjuk, a gohome miatt.
                                            savedMovesValue[globalCounter] = -1000; //lekell cserélni az adott motorfordulatra. itt is ellentétes érték kell.
                                            globalCounter++;
                                            stepCount++;
                                            Thread.Sleep(10000); //10mp t vár h befejezze a müveletet, változtatni kell.
                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Nxt move error.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                    }
                                    else
                                    {
                                        //itt megkell hivni a koordináta számítást h pontosan hol a labda.
                                        ballCounter++;
                                        stepCount = 0;
                                        listBox1.Items.Add(ballCounter + ". ball found!");
                                        listBox1.TopIndex = listBox1.Items.Count - 1;
                                        try
                                        {
                                            robot.robotic_arm_close_open(false);
                                            listBox1.Items.Add("Ball catched.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                            Thread.Sleep(10000);

                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Ball catching error");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                        goHome();
                                    }
                                }
                                break;

                            case 4:
                                {
                                    if (colorFindResult == false)
                                    {
                                        try
                                        {
                                            robot.forward(20, 100, 1000); // 1000-t cserélni kell helyes értékre.
                                            savedMovesType[globalCounter] = "backward"; //ellentétes értéket adjuk, a gohome miatt.
                                            savedMovesValue[globalCounter] = -1000; //lekell cserélni az adott motorfordulatra. itt is ellentétes érték kell.
                                            globalCounter++;
                                            stepCount++;
                                            Thread.Sleep(10000); //10mp t vár h befejezze a müveletet, változtatni kell.
                                            goHome(); // pálya végére ért.
                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Nxt move error.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                    }
                                    else
                                    {
                                        //itt megkell hivni a koordináta számítást h pontosan hol a labda.
                                        ballCounter++;
                                        stepCount = 0;
                                        listBox1.Items.Add(ballCounter + ". ball found!");
                                        listBox1.TopIndex = listBox1.Items.Count - 1;
                                        try
                                        {
                                            robot.robotic_arm_close_open(false);
                                            listBox1.Items.Add("Ball catched.");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                            Thread.Sleep(10000);

                                        }
                                        catch (Exception)
                                        {
                                            listBox1.Items.Add("Ball catching error");
                                            listBox1.TopIndex = listBox1.Items.Count - 1;
                                        }
                                        goHome();
                                    }
                                }
                                break;
                             
                                /*itt az ellenkező stringet kell majd belerakni vagyis ha felmegy (up)
                                akkor down -t kell berakni a tömbe, hogy a gohome -nál már ne kelljen 
                                cserélgetni, mivel ott az ellentétes írány fog kelleni.*/                           
                           
                        }
                }
                    listBox1.Items.Add("NXT Finished");
                    listBox1.TopIndex = listBox1.Items.Count - 1;
                    stopCamera();
                }
                catch
                {
                    listBox1.Items.Add("Error");
                    listBox1.TopIndex = listBox1.Items.Count - 1;
                }
        }
        #endregion

        #region Connect Button

        private void button5_Click(object sender, EventArgs e)
        {
            StreamReader rs;

            bool nxtConnected_1 = false; //felső nxt
            bool nxtConnected_2 = false; //alsó nxt

            try
            {
                rs = new StreamReader("comPort.nxt");
                while (rs.EndOfStream == false)
                {
                    cPTemp = rs.ReadLine();
                }
                rs.Close();

                cP = cPTemp.Split(',');
            }
            catch
            {
                listBox1.Items.Add("COM PORT reading error!");
                listBox1.TopIndex = listBox1.Items.Count - 1;
            }

            robot = new Control(cP[0], cP[1]);

            listBox1.Items.Add("Nxt_1 COM PORT: " + cP[0] + ", Nxt_2 COM PORT: " + cP[1]);
            listBox1.TopIndex = listBox1.Items.Count - 1;

            try
            {
                listBox1.Items.Add("Connecting to Nxt_1 and Nxt_2...");
                listBox1.TopIndex = listBox1.Items.Count - 1;

                robot.Connect();
                
                //felső nxt
                if (robot.nxtConnectionStatusFront() == true)
                {
                    nxtConnected_1 = true;

                    listBox1.Items.Add("Nxt_1 is connected!");
                    listBox1.TopIndex = listBox1.Items.Count - 1;

                }
                else
                {
                    listBox1.Items.Add("Nxt_1 is not connected!");
                    listBox1.TopIndex = listBox1.Items.Count - 1;

                }

                //alsó nxt
                if (robot.nxtConnectionStatusBack() == true)
                {
                    nxtConnected_2 = true;

                    listBox1.Items.Add("Nxt_2 is connected!");
                    listBox1.TopIndex = listBox1.Items.Count - 1;

                }
                else
                {
                    listBox1.Items.Add("Nxt_2 is not connected!");
                    listBox1.TopIndex = listBox1.Items.Count - 1;

                }

                //felső-alsó
                if (nxtConnected_1 == true && nxtConnected_2 == true)
                {
                    listBox1.Items.Add("Connected! Ready to go!");
                    listBox1.TopIndex = listBox1.Items.Count - 1;

                    robot.greenLight();
                }
            }
            catch
            {
                listBox1.Items.Add("Connection failed to Nxt!");
                listBox1.TopIndex = listBox1.Items.Count - 1;
            }

        
        }

        #endregion

        #region GO

        private void button6_Click(object sender, EventArgs e)
        {
            double moveFwBw = Convert.ToDouble(textBox1.Text);
            double moveRightLeft = Convert.ToDouble(textBox2.Text);
            double moveUpDown = Convert.ToDouble(textBox3.Text);

            robot.calcTachoFwBw(moveFwBw);
            robot.calcTachoRightLeft(moveRightLeft);
            robot.calcTachoUpDown(moveUpDown);
            
        }

        #endregion


        #region Robot in position

        private void button9_Click(object sender, EventArgs e)
        {
            robot.setValues();
        }

        #endregion
    }
     
}
